/*
 * main.cpp
 *
 *  Created on: Apr 14, 2012
 *      Author: Victor Nidens
 */

#include <iostream>
#include <arpa/inet.h>
#include "string.h"
#include "protocol/protocol.h"
#include "nxtconst.h"
#include "nxt.h"
#include <fstream>

#include <mysql/mysql.h>

using namespace std;

int main(int argc, char **argv){

	int tcpSockFD = -1;

	struct sockaddr_in sock;

	int res = 0;

	int angle;

	struct timeval masterLastReadTime;
	struct timeval slaveLastReadTime;

	uint8 buffer[128];
	uint8 receiveBuffer[128];

	ExchangePacket pingPacket;
	ExchangePacket pack;
	ExchangePacket motorPacket;

	uint8 i2cCalibrateCommand[4] = {0x02, 0x41, 0x43, 0};
	uint8 i2cReadCommand[3] = {0x02, 0x42, 0};

	uint8 prevAngle = 0;
	uint8 currAngle = 0;

	memset(&sock, 0, sizeof(sock));

	memset(&pack, 0, sizeof(pack));

	sock.sin_family = AF_INET;
	sock.sin_addr.s_addr = inet_addr("127.0.0.1");
	sock.sin_port = htons(2200);

	tcpSockFD = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

	res = connect(tcpSockFD, (sockaddr*)&sock, sizeof(sock));

	if(res != 0){
		cerr << "Can't connect to the server" << endl;
		return 0;
	}

	memset(&pingPacket, 0, sizeof(pingPacket));

	constructCommand(&pingPacket.command, COMMAND_PING, "Regulator1", 0, 0, PROTOCOL_TCP);

	fillSourceInfo(&pingPacket.source, (uint8*)"\x7F\x00\x00\x01", "Regulator1", PROTOCOL_TCP);

	memset(buffer, 0, sizeof(buffer));

	memcpy(buffer, &pingPacket, NETWORK_PACKET_SIZE);

	res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

	constructCommand(&pack.command, COMMAND_INIT_SENSOR_REQUEST, "Client1", 0, 0, PROTOCOL_TCP);
	fillSourceInfo(&pack.source, (uint8*)"\x7F\x00\x00\x01", "Regulator1", PROTOCOL_TCP);

	pack.deviceData.dataFormat = FORMAT_RAW;
	memcpy(pack.deviceData.deviceName, "NXT", 3);
	pack.deviceData.port = NXT_SENSOR_1; //port 1
	pack.deviceData.sensor = NXT_LOW_SPEED;
	pack.deviceData.dataSize = 6;

	pack.deviceData.data[0] = NXT_RAW_MODE;
	pack.deviceData.data[1] = 3;
	pack.deviceData.data[2] = 0;
	memcpy(pack.deviceData.data + 3, i2cCalibrateCommand, 3);

	memset(buffer, 0, sizeof(buffer));

	memcpy(buffer, &pack, NETWORK_PACKET_SIZE);

	res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

	fillSourceInfo(&pack.source, &pack.source);
	memcpy(pack.deviceData.deviceName, "NXT1", 4);
	memset(buffer, 0, sizeof(buffer));
	memcpy(buffer, &pack, NETWORK_PACKET_SIZE);
	res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

	SensorState sensor;
	ExchangePacket receivedPacket;
	DigitalSensorReadRespose digitalRes;

	memset(&pack, 0, sizeof(pack));

	constructCommand(&pack.command, COMMAND_READ_SENSOR_REQUEST, "Client1", 0, 0, PROTOCOL_TCP);
	fillSourceInfo(&pack.source, (uint8*)"\x7F\x00\x00\x01", "Regulator1", PROTOCOL_TCP);

	pack.deviceData.dataFormat = FORMAT_RAW;
	memcpy(pack.deviceData.deviceName, "NXT", 3);
	pack.deviceData.port = NXT_SENSOR_1; //port 1
	pack.deviceData.sensor = NXT_LOW_SPEED;
	pack.deviceData.dataSize = 5;

//	pack.deviceData.data[0] = NXT_RAW_MODE;
	pack.deviceData.data[0] = 2;
	pack.deviceData.data[1] = 2;
	memcpy(pack.deviceData.data + 2, i2cReadCommand, 2);

	memset(&motorPacket, 0, sizeof(motorPacket));

	fillSourceInfo(&motorPacket.source, (uint8*)"\x7F\x00\x00\x01", "Regulator1", PROTOCOL_TCP);
	constructCommand(&motorPacket.command, COMMAND_WRITE_MOTOR_REQUEST, "Client1", 0, 0, PROTOCOL_TCP);

	motorPacket.deviceData.dataFormat = FORMAT_RAW;
	memcpy(motorPacket.deviceData.deviceName, "NXT", 3);
	motorPacket.deviceData.sensor = NXT_MOTOR_ALL;

	motorPacket.deviceData.dataSize = 7;
	motorPacket.deviceData.data[0] = 50;
	motorPacket.deviceData.data[1] = NXT_MOTOR_ON | NXT_REGULATED;
	motorPacket.deviceData.data[2] = NXT_REGULATION_MODE_MOTOR_SPEED;
	motorPacket.deviceData.data[3] = 0;
	motorPacket.deviceData.data[4] = NXT_MOTOR_RUN_STATE_RUNNING;
	uint32 tacho = 0;
	memcpy(motorPacket.deviceData.data + 5, &tacho, 4);

	FILE *masterAngleLog = fopen("~/master_angle.txt", "w");
	FILE *slaveAngleLog = fopen("~/slave_angle.txt", "w");

	while (1) {

		prevAngle = currAngle;

		memset(receiveBuffer, 0, sizeof(receiveBuffer));

		//read angle sensor value of master and slave devices
		fillSourceInfo(&pack.source, &pack.source);
		memset(pack.deviceData.deviceName, 0, 8);
		memcpy(pack.deviceData.deviceName, "NXT", 3);
		memset(buffer, 0, sizeof(buffer));
		memcpy(buffer, &pack, NETWORK_PACKET_SIZE);
		res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

		fillSourceInfo(&pack.source, &pack.source);
		memset(pack.deviceData.deviceName, 0, 8);
		memcpy(pack.deviceData.deviceName, "NXT1", 4);
		memset(buffer, 0, sizeof(buffer));
		memcpy(buffer, &pack, NETWORK_PACKET_SIZE);
		res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

		res = read(tcpSockFD, receiveBuffer, NETWORK_PACKET_SIZE);

		memset(&digitalRes, 0, sizeof(digitalRes));
		memset(&receivedPacket, 0, sizeof(receivedPacket));

		memcpy(&receivedPacket, receiveBuffer, sizeof(receivedPacket));

		memcpy(&digitalRes, receivedPacket.deviceData.data, sizeof(digitalRes));

		currAngle = digitalRes.data[0];
		if(memcmp(receivedPacket.deviceData.deviceName, "NXT1", 4)){
			currAngle = digitalRes.data[0];
			angle = (int)digitalRes.data[0]*2;
			fprintf(masterAngleLog, "%i\n", angle);
			fflush(masterAngleLog);
		} else {
			angle = (int)digitalRes.data[0]*2;
			fprintf(slaveAngleLog, "%i\n", angle);
			fflush(slaveAngleLog);
		}

		res = read(tcpSockFD, receiveBuffer, NETWORK_PACKET_SIZE);

		memset(&digitalRes, 0, sizeof(digitalRes));
		memset(&receivedPacket, 0, sizeof(receivedPacket));

		memcpy(&receivedPacket, receiveBuffer, sizeof(receivedPacket));

		memcpy(&digitalRes, receivedPacket.deviceData.data, sizeof(digitalRes));

		if(memcmp(receivedPacket.deviceData.deviceName, "NXT1", 4)){
			currAngle = digitalRes.data[0];
			angle = (int)digitalRes.data[0]*2;
			fprintf(masterAngleLog, "%i\n", angle);
			fflush(masterAngleLog);
		} else {
			angle = (int)digitalRes.data[0]*2;
			fprintf(slaveAngleLog, "%i\n", angle);
			fflush(slaveAngleLog);
		}

		if(currAngle <= 30 || currAngle >= 150){	//main code
			if (currAngle > prevAngle || (currAngle > 0 && currAngle < 180 && prevAngle > 180 && prevAngle < 360)) {		//main code
				//send first motor control command
				memset(buffer, 0, sizeof(buffer));
				fillSourceInfo(&motorPacket.source, &motorPacket.source);

				motorPacket.deviceData.port = NXT_MOTOR_A;
				motorPacket.deviceData.data[0] = -45;		//main code

				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT", 3);

				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);

				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				fillSourceInfo(&motorPacket.source, &motorPacket.source);
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT1", 4);
				memset(buffer, 0, sizeof(buffer));
				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);
				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				//send second motor control command
				memset(buffer, 0, sizeof(buffer));
				fillSourceInfo(&motorPacket.source, &motorPacket.source);

				motorPacket.deviceData.port = NXT_MOTOR_B;
				motorPacket.deviceData.data[0] = 45;	//main code
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT", 3);

				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);

				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				fillSourceInfo(&motorPacket.source, &motorPacket.source);
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT1", 4);
				memset(buffer, 0, sizeof(buffer));
				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);
/				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);
			} else if (currAngle < prevAngle || (currAngle > 180 && currAngle < 360 && prevAngle > 0
							&& prevAngle < 180)) {
				//send first motor control command
				memset(buffer, 0, sizeof(buffer));
				fillSourceInfo(&motorPacket.source, &motorPacket.source);

				motorPacket.deviceData.port = NXT_MOTOR_A;
				motorPacket.deviceData.data[0] = 45;	//main code

				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT", 3);

				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);

				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				fillSourceInfo(&motorPacket.source, &motorPacket.source);
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT1", 4);
				memset(buffer, 0, sizeof(buffer));
				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);
				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				//send second motor control command
				memset(buffer, 0, sizeof(buffer));
				fillSourceInfo(&motorPacket.source, &motorPacket.source);

				motorPacket.deviceData.port = NXT_MOTOR_B;
				motorPacket.deviceData.data[0] = -45;		//main code
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT", 3);

				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);

				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

				fillSourceInfo(&motorPacket.source, &motorPacket.source);
				memset(motorPacket.deviceData.deviceName, 0, 8);
				memcpy(motorPacket.deviceData.deviceName, "NXT1", 4);
				memset(buffer, 0, sizeof(buffer));
				memcpy(buffer, &motorPacket, NETWORK_PACKET_SIZE);
				res = send(tcpSockFD, buffer, NETWORK_PACKET_SIZE, 0);

			}
		}
	}

	return 0;
}
